#include "MP_BasicFunction.h"

/**
 * @brief �ѿ����������������㺯��
 * @param p0  ��һ����
 * @param p1  �ڶ�����
 * @return dist ����������
 */
double CarPointDistance(CAR_POINT p0, CAR_POINT p1)
{
	double dist = 0;
	dist += (p0.dX - p1.dX) * (p0.dX - p1.dX);
	dist += (p0.dY - p1.dY) * (p0.dY - p1.dY);
	dist = sqrt(dist);
	return dist;
}

/**
 * @brief �����������������㺯��
 * @param point0  ��һ����
 * @param point1  �ڶ�����
 * @return result ����������
 */
double GeoPointDistance(GEO_POINT point0, GEO_POINT point1)
{
	// �Ե�һ����Ϊԭ�㽨������������ϵ���õ��ڶ������ĵѿ�������
	double tempDouble = M_PI * 6378245.0 / 180;
	double xy[2];
	xy[0] = (point1.dLat - point0.dLat) * tempDouble;
	xy[1] = (point1.dLon - point0.dLon) * tempDouble * cos(point1.dLat * M_PI / 180);
	// ���ѿ�������ϵ����ת��Ϊ����
	return sqrt(xy[0] * xy[0] + xy[1] * xy[1]);
}

/**
 * @brief �ѿ�������ϵת��γ��
 * @param lon0     �ѿ�������ϵԭ�㾭��
 * @param lat0     �ѿ�������ϵԭ��γ��
 * @param x        �ѿ���ϵx����
 * @param y        �ѿ���ϵy����
 * @param lonLat1  ת��֮���ľ�γ��
 * @return None
 */
void Car2Geo(double lon0, double lat0, double x, double y, double lonLat1[2])
{
	double tempDouble1 = 180 / (M_PI * 6378245.0);
	double tempDouble2 = x * tempDouble1 + lat0;
	double tempDouble3 = 1 / (cos(tempDouble2 * M_PI / 180));
	lonLat1[0] = y * tempDouble1 * tempDouble3 + lon0;
	lonLat1[1] = tempDouble2;
}

/**
 * @brief ��γ��ת�ѿ�������
 * @param lon0 �ѿ�������ϵԭ�㾭��
 * @param lat0 �ѿ�������ϵԭ��γ��
 * @param lon1 ����
 * @param lat1 γ��
 * @param xy   ת��֮���ĵѿ�������
 * @return None
 */
void Geo2Car(double lon0, double lat0, double lon1, double lat1, double xy[2])
{
	double tempDouble = M_PI * 6378245.0 / 180;
	xy[0] = (lat1 - lat0) * tempDouble;
	xy[1] = (lon1 - lon0) * tempDouble * cos(lat1 * M_PI / 180);
}

double GeoDirectionAngle(double lon0, double lat0, double lon1, double lat1)
{
	double xy[2] = { 0 };
	Geo2Car(lon0, lat0, lon1, lat1, xy);
	return atan2(xy[1], xy[0]);
}

/**
 * @brief ��ά����ƫ����ת����
 * @param yaw    ƫ����
 * @param rotMat ��ת����
 * @return None
 */
void PosiYawRotateMatrix2D(double yaw, double rotMat[2][2])
{
	rotMat[0][0] = cos(yaw); rotMat[0][1] = -sin(yaw);
	rotMat[1][0] = sin(yaw); rotMat[1][1] = cos(yaw);
}

/**
 * @brief ������ʱ���нǼ��㺯��
 * @param point0 ����0
 * @param point1 ����1
 * @return Angle ������0������1��ʱ��ת��
 */
double CounterclockwiseAngle(CAR_POINT point0, CAR_POINT point1)
{
	double DotProduct = point0.dX * point1.dX + point0.dY * point1.dY;
	double CrossProduct = point0.dX * point1.dY + point0.dY * point1.dX;
	double Angle = atan2(DotProduct, CrossProduct);
	return Angle;
}


/**
 * @brief �������˺���
 * @param vec1    ����1
 * @param vec2    ����2
 * @return result ���˽�����ֵ
 */
double VectorCross(double vec1[2], double vec2[2])
{
	return vec1[0] * vec2[1] - vec1[1] * vec2[0];
}
